package kit.edu.lego.gruppe3.levels.labyrinth;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import lejos.nxt.LCD;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class FindLeftWall implements Behavior
{

	
	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();

	private int minDistance = 15;
	private int maxDistance = 22;
	
	private boolean distanceToClose = false;
	private boolean distanceToFar = false;
	
	private int distance;

	@Override
	public void action()
	{
		LCD.drawString("FRW", 0,1);


		if (distanceToClose)
		{
			robot.setTravelSpeed(5);
			robot.steer(-60);
			
		}
		
		if (distanceToFar)
		{
			robot.setTravelSpeed(5);
			robot.steer(60);
		}

	}

	@Override
	public void suppress() 
	{

	}

	@Override
	public boolean takeControl()
	{
		distance = ComponentsRepository.getInstance().getSonicSensor().getDistance();
		distanceToClose = minDistance > distance;
		distanceToFar = maxDistance < distance;
		return (LevelSelector.getInstance().isLabyrinth() || LevelSelector.getInstance().isDore()) &&
			 (distanceToClose || distanceToFar);
	}

}